#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <Arduino.h>
#include <WiFi.h>
#include "imu.h"
#include "wifi_task.h"
const char *wifi_SSID="YXDZ_ESP321";  //保存AP的名称信息
const char *wifi_Password="ESP321234";  //保存AP的密码信息
const char* udpAddress = "192.168.4.2"; // 目标设备的IP地址
const int udpPort = 7890;
unsigned int localUdpPort = 2333; //本地端口号
const uint16_t serverPort = 1234; //端口
#define WIRE_PORT Wire 
// UDP实例
WiFiUDP udp;
WiFiClient client;
void wifi_set(){
  // 启动WiFi为AP模式
  WiFi.softAP(wifi_SSID, wifi_Password);

  // 输出AP的相关信息到串口
  Serial.print("\nESP32建立的WiFi名称为:");
  Serial.println(wifi_SSID);
  Serial.print("ESP32建立WiFi的IP地址为:");
  Serial.println(WiFi.softAPIP());

  // 启动UDP监听
  udp.begin(localUdpPort);
  Serial.printf("UDP监听端口:%d\n", localUdpPort);
  WIRE_PORT.begin();
  WIRE_PORT.setClock(400000);
}
void wifitask(void *pvParameters){
  double imu_x,imu_y,imu_z;
  while(1){

  // 将三个double变量打包成一个缓冲区
  double IMU_data[3] = {imu_x, imu_y, imu_z};
  int bytes_to_send = sizeof(IMU_data);

  // 开始UDP包
  // int beginPacketStatus = udp.beginPacket(udpAddress, udpPort);
  // if (beginPacketStatus == 1) { // 成功开始包
  //   // 写入数据到UDP包
  //   int written = udp.write((uint8_t*)imu_data, bytes_to_send);
  //   if (written == bytes_to_send) {
  //     // 成功发送
  //     udp.endPacket();
  //     //Serial.println("IMU数据发送成功");
  //   } else {
  //     // 发送过程中出错
  //     udp.endPacket();
  //     Serial.println("IMU数据发送失败:写入数据不完整");
  //   }
  // } else {
  //   Serial.println("IMU数据发送失败:无法开始UDP包");
  // }

  // // 检查是否有收到UDP包
  // int packetSize = udp.parsePacket();
  // if (packetSize > 0) {
  //   // 为接收到的数据分配缓冲区
  //   uint8_t* incomingData = new uint8_t[packetSize];
  //   int len = udp.read(incomingData, packetSize);
  //   if (len > 0) {
  //     // 处理接收到的数据
  //     // 假设数据为二进制格式的double数组
  //     int num_doubles = len / sizeof(double);
  //     if (num_doubles == 4) {
  //       // 接收到的是四个double的控制信号
  //       double control_signals[4];
  //       memcpy(control_signals, incomingData, 4 * sizeof(double));
        /*
        Serial.println("接收到控制信号：");
        for (int i = 0; i < 4; i++) {
          Serial.printf("控制信号 %d: %lf\n", i + 1, control_signals[i]);
        }
        */
        // 在这里添加处理控制信号的代码
        // can1 = control_signals[0];
        // can2 = control_signals[1];
        // can3 = control_signals[2];
        // can4 = control_signals[3];
      }
      // else if (num_doubles == 3) {
      //   // 接收到的是三个double的信号
      //   double xyz[3];
      //   memcpy(xyz, incomingData, 3 * sizeof(double));
      //   dbx=xyz[0];
      //   dby=xyz[1];
      //   dbz=xyz[2];
      //   Serial.println("接收到动捕坐标:");
      //   for (int i = 0; i < 3; i++) {
      //     Serial.printf("动捕坐标 %d: %lf\n", i + 1, xyz[i]);
      //   }
      // }
      // else if (num_doubles == 9) {
      //   // 接收到的是九个double的PID参数
      //   double pid_params[9];
      //   memcpy(pid_params, incomingData, 9 * sizeof(double));
      //   Serial.println("接收到PID参数:");
      //   for (int i = 0; i < 9; i++) {
      //     Serial.printf("PID参数 %d: %lf\n", i + 1, pid_params[i]);
      //   }
      //   // 在这里添加 serial exception error occurred: Cannot configure port, something went wrong. Original message: OSError(22, 'ָ�������ڵ��豸��', None, 433)
      //   gzkp = pid_params[0];
      //   gzki = pid_params[1];
      //   gzkd = pid_params[2];
      //   yawkp = pid_params[3];
      //   yawki = pid_params[4];
      //   yawkd = pid_params[5];
      //   fykp = pid_params[6];
      //   fyki = pid_params[7];
      //   fykd = pid_params[8];

      // }
  //     else {
  //       Serial.printf("接收到未知长度的数据：%d 字节\n", len);
  //     }
  //   }
  //   // 释放动态分配的内存
  //   delete[] incomingData;
    
  // }  
  // }
  vTaskDelay(10 / portTICK_PERIOD_MS);  // 延时10毫秒
}